/** 
 * @file Main.cpp
 * @author Kevin Vancauwenbergh
 * @brief Contains the entry function for the project
 */

#include "lowLevel/LowLevel.h"
#include "wifi/Client.h"
#include "wifi/RobotData.h"
#include "strategy/CmdGenerator.h"
#include "strategy/Cmd.h"
#include "strategy/Point.h"
#include "strategy/sendcmd.h"
#include "strategy/KortstePad.h"
#include <string.h>
#include <vector>

using namespace std;

/** @brief The entry function for the project 
 *  @return 0 is no error occured	
 * **/
int main(int argc, char *argv[]) {
	// Initialize wifistuff
	RobotData *robA = new RobotData(); 
	RobotData *robB = new RobotData();
	vector<Point> obstacles;

	Client *client = Client::getInstance();
	client->start();

	printf("Wifi client gestart\n\n");

	// Loop runs as long as it has to 
	bool running = true;
	
	// Read wifi frame
	// Setup Robotdata
	//vector<Point> *obstacleData = new vector<obstacleData>;
	//RobotData *robotData = new RobotData(10,10,45,20,20,30,30);
	
	while(!client->receive(robA,robB,obstacles)){};
	robA->print();
	//Point pointStart (10,10);
	//Point pointStop (20,20);
	//vector<Point> obstacleData;

	// Calculate shortest path
	Point pointStart(robA->get_robotX(), robA->get_robotY());
	Point pointStop(robA->get_blikjeX(), robA->get_blikjeX());
	printf("rob x: %d, roby: %d, blik x: %d, blik y: %d\n", robA->get_robotX(), robA->get_robotY(), robA->get_blikjeX(), robA->get_blikjeY());
	KortstePad pad(pointStart, pointStop, obstacles);
	vector<Point> weg = pad.getWeg();
	printf("Kortste pad gegenereerd\n");
	
	// Real list
	vector<Point> realList(10);
	for(int i = 0; i < realList.size(); i++)
	{
		Point p (rand() % 100 + 1, rand() % 100 + 1);
		realList.at(i) = p;
	}

	// Generate command list
	vector<Cmd> cmdList;
	CmdGenerator* gen = new CmdGenerator();

	cmdList = gen->generateList(weg, robA->get_angle());
	//cmdList = gen->generateList(realList, 100);

	sendcmd* s = new sendcmd(robA, client);

	for(int i = 0; i < cmdList.size(); i++)
	{
		printf("Sending commando %d\n", i);
		s->drive(cmdList.at(i).getDistance(), cmdList.at(i).getAngle());
	}
	
	return 0;
}
